#include "CuspFilter.h"
#include "DataStruct.h"

CuspFilter::CuspFilter()
{
    this->N = 5;
    this->threshold_X = 20;// todo diff大的时候的倍数
    this->threshold_Z = 50;
    this->temp_data_state = false;
    this->target_out = true;
    this->target_lose = true;
    this->count_frame_out = 0;
    this->count_frame_lose = 0;
    dataBuff.clear();
};

void CuspFilter::setFilter(int Num, int threshold)
{
    this->N = Num;
    this->threshold_X = threshold;
    this->threshold_Z = threshold;
    this->target_out = true;
    this->target_lose = true;
    this->temp_data_state = false;
    this->count_frame_out = 0;
    this->count_frame_lose = 0;
    dataBuff.clear();
};

void CuspFilter::buffClear()
{
    this->temp_data_state = false;
    this->target_out = true;
    this->target_lose = true;
    this->count_frame_out = 0;
    this->count_frame_lose = 0;
    dataBuff.clear();
};

CuspFilter::~CuspFilter(){

};

void CuspFilter::filter(DataStruct &ture_data, DataStruct &send_data)
{
    if (ture_data.Flag != 0) //有识别到目标
    {
        send_data.Y=ture_data.Y;
        cout << "                                                0\n";
        count_frame_lose = 0;
        if (temp_data_state == false) //flase代表当前存储区没有数据,第一帧数据存入后返回
        {
            cout << "1";
            temp_data_state = true; //true代表存储区已经有数据
            target_out = false;     //false代表目标没有离开感兴趣区间
            target_lose = false;    //false代表目标没有丢失
            dataBuff.push_back(ture_data);
            temp_data = ture_data;
            send_data = ture_data;
            return;
        }
        else //数据缓冲区帧数大于等于2
        {
            cout << "2";
            mid_X = 0;
            mid_Z = 0;
            int buffSize = dataBuff.size(); //将缓冲区限制在六帧
            for (int i = 0; i < buffSize; i++)
            {
                mid_X += dataBuff[i].X;
                mid_Z += dataBuff[i].Z;
            }
            mid_X = mid_X / buffSize;
            mid_Z = mid_Z / buffSize;
            if (buffSize == 4)
            {                                     //删除第一个元素
                dataBuff.erase(dataBuff.begin()); //存储帧数到达7帧时，扔掉第一帧
                buffSize--;
            }

            if (abs(temp_data.X - mid_X) < 30 || abs(temp_data.Z - mid_Z) < 100)//diff算小的时候 判断
            {

                if (target_lose == true) //在4帧内丢失了目标，重新找到后的目标又超出了感兴趣区域
                {
                    target_lose = false;
                    count_frame_out = count_frame_lose;
                    count_frame_lose = 0;
                    count_frame_out++;
                }
                else //六帧内没有丢目标，但目标超出了感兴趣区域
                {
                    count_frame_out++;
                }

                if (count_frame_out > N) //连续五帧检测到的目标检测到的目标都在感兴趣区间外，开始重新寻找目标
                {
                    buffClear();
                    send_data = ture_data;
                    return;
                }
                if (target_out == false) //目标首次超出感兴趣区间
                {
                    if ((ture_data.X - temp_data.X) > 100)
                        send_data.X = ture_data.X + 70;
                    else if ((ture_data.X - temp_data.X) < -100)
                        send_data.X = ture_data.X - 70;

                    if ((ture_data.Z - temp_data.Z) > 300)
                        send_data.Z = ture_data.Z + 200;
                    else if ((ture_data.Z - temp_data.Z) < -300)
                        send_data.Z = ture_data.Z - 200;
                }
            }

            else if (abs(ture_data.X - temp_data.X) > abs(temp_data.X - mid_X) * this->threshold_X ||
                     abs(ture_data.Z - temp_data.Z) > abs(temp_data.Z - mid_Z) * this->threshold_Z) //如果当前帧与前一帧中心点位置发生突变，扔掉
            {
                if (target_lose == true) //在4帧内丢失了目标，重新找到后的目标又超出了感兴趣区域
                {
                    target_lose = false;
                    count_frame_out = count_frame_lose;
                    count_frame_lose = 0;
                    count_frame_out++;
                }
                else //六帧内没有丢目标，但目标超出了感兴趣区域
                {
                    count_frame_out++;
                }

                if (count_frame_out > N) //连续五帧检测到的目标检测到的目标都在感兴趣区间外，开始重新寻找目标
                {
                    buffClear();
                    send_data = ture_data;
                    return;
                }
                if (target_out == false) //目标首次超出感兴趣区间
                {
                    if (abs(ture_data.X - temp_data.X) > abs(temp_data.X - mid_X) * this->threshold_X)
                    {
                        cout << "x:" << (ture_data.X - temp_data.X) << endl;
                        if (temp_data.X > ture_data.X)
                            send_data.X = ture_data.X - (temp_data.X - ture_data.X);
                        else
                            send_data.X = ture_data.X + (ture_data.X - temp_data.X);
                    }
                    if (abs(ture_data.Z - temp_data.Z) > abs(temp_data.Z - mid_Z) * this->threshold_Z)
                    {
                        //cout << "z:" << (ture_data.Z - temp_data.Z) << endl;
                        if (temp_data.Z > ture_data.Z)
                            send_data.Z = ture_data.Z - (temp_data.Z - ture_data.Z);
                        else
                            send_data.Z = ture_data.Z + (ture_data.Z - temp_data.Z);
                    }
                }
                else
                {
                    send_data = temp_data;
                    return;
                }
            }
            else
            {
                dataBuff.push_back(ture_data);                 //在感兴趣区间获取到目标
                if (target_out == true || target_lose == true) //六帧以内重新找回目标,temp_data保持上一次的有效值
                {
                    target_out = false;
                    target_lose = false;
                    count_frame_out = 0;
                    count_frame_lose = 0;
                    send_data = ture_data;
                    temp_data = ture_data;
                    return;
                }
                else //六帧内未丢失过目标
                {
                    temp_data = ture_data;
                    send_data = ture_data;
                    return;
                }
            }
        }
    }
    else //没有识别到装甲片
    {
        if (temp_data_state == false) //缓冲区没有数据，返回输入值
        {
            send_data = ture_data;
            return;
        }

        count_frame_lose++;

        if (count_frame_lose < N) //丢失目标的帧数小于N
        {
            target_lose = true;
            if (target_out == true) //如果之前目标是target_out，将target_out计数次数加到target_lose上
            {
                count_frame_lose = count_frame_out;
                count_frame_out = 0;
                target_out = false;
            }
            send_data = temp_data; //目标完全丢失，六帧内发送上一次的有效数据
            return;
        }
        else //彻底丢失目标，开始重新寻找
        {
            buffClear();
            send_data = ture_data;
            return;
        }
    }
};
